State estimation device and state estimation method

ABSTRACT

By a state estimation device or a state estimation method, image data is read, a feature point included in the image data is extracted, the feature point is tracked, a position, a velocity, or an attitude of a mobile object is calculated based on inertia data, a bias error of an inertial measurement unit is calculated, correction data is calculated by removing the bias error from the inertia data, and a state including at least one of the position, the velocity, or the attitude of the mobile object is estimated based on the correction data.

CROSS REFERENCE TO RELATED APPLICATION

The present application claims the benefit of priority from JapanesePatent Application No. 2022-097471 filed on Jun. 16, 2022. The entiredisclosure of the above application is incorporated herein by reference.

TECHNICAL FIELD

The present disclosure relates to a state estimation device and a stateestimation method for estimating a state including at least one of aposition, a speed, or an attitude of a mobile object.

BACKGROUND

Conventionally, there is a technology such as a visual inertial odometry(VIO) that uses a camera and an inertial measurement unit (so-calledIMU) to accurately estimates multiple parameters by a nonlinearleast-squares method called bundle adjustment. For example, there is acomparative technology of estimating the position, the attitude, thevelocity of a mobile object, and a bias error of an inertial measurementunit by the VIO.

SUMMARY

By a state estimation device or a state estimation method, image data isread, a feature point included in the image data is extracted, thefeature point is tracked, a position, a velocity, or an attitude of amobile object is calculated based on inertia data, a bias error of aninertial measurement unit is calculated, correction data is calculatedby removing the bias error from the inertia data, and a state includingat least one of the position, the velocity, or the attitude of themobile object is estimated based on the correction data.

BRIEF DESCRIPTION OF DRAWINGS

FIG. 1 is a schematic configuration diagram of a state estimation deviceaccording to a first embodiment.

FIG. 2 is an explanatory diagram illustrating a capture unit used by thestate estimation device.

FIG. 3 is an explanatory diagram illustrating an inertial measurementunit used by the state estimation device.

FIG. 4 is an explanatory diagram illustrating an overview of visualinertia odometry.

FIG. 5 is an explanatory diagram illustrating a residual related to animage.

FIG. 6 is an explanatory diagram illustrating a residual related to theinertial measurement unit.

FIG. 7 is an explanatory diagram illustrating a residual related toprior information.

FIG. 8 is an explanatory diagram illustrating an angular velocityestimated from the angular velocity measured by a gyro sensor and theattitude obtained by the visual inertia odometry.

FIG. 9 is an explanatory diagram illustrating temporal changes in biaserror included in inertia data.

FIG. 10 is an explanatory diagram illustrating a state estimation deviceaccording to the first embodiment.

FIG. 11 is an explanatory diagram illustrating estimation results of avehicle attitude based on correction data calculated from the bias errorobtained by the visual inertia odometry and the inertia data.

FIG. 12 is an explanatory diagram illustrating a state estimation deviceaccording to a second embodiment.

FIG. 13 is an explanatory diagram illustrating a state estimation deviceaccording to a third embodiment.

FIG. 14 is an explanatory diagram illustrating an estimation method of avehicle attitude in the state estimation device according to the thirdembodiment.

FIG. 15 is an explanatory diagram illustrating a method of calculatingand obtaining a vehicle attitude angle from a sensor output of anacceleration sensor.

FIG. 16 is an explanatory diagram illustrating a difficulty in anabnormal situation.

FIG. 17 is an explanatory diagram illustrating a state estimation deviceaccording to a fourth embodiment.

FIG. 18 is an explanatory diagram illustrating behavior under anabnormal situation.

FIG. 19 is an explanatory diagram illustrating a state estimation deviceaccording to a fifth embodiment.

FIG. 20 is an explanatory diagram illustrating a state estimation deviceaccording to a sixth embodiment.

FIG. 21 is an explanatory diagram illustrating a state estimation deviceaccording to a seventh embodiment.

FIG. 22 is an explanatory diagram illustrating a state estimation deviceaccording to an eighth embodiment.

FIG. 23 is an explanatory diagram illustrating a state estimation deviceaccording to a ninth embodiment.

DETAILED DESCRIPTION

As the result of detailed study by the present inventors, it has beenfound that, when the position, the attitude, the speed of a mobileobject such as a vehicle and the bias error of the inertial measurementunit are estimated, an estimation error of the attitude change within apredetermined time from a start time of these estimation is large. Apossible reason for this is that an image output by the camera issusceptible to motion blur and movement of surrounding objects, and theerror in the attitude obtained based on image data increases in a shortperiod. It should be noted that these have been found by the detailedstudy by the present inventors.

One example of the present disclosure provides a state estimation deviceand a state estimation method capable of improving an estimationaccuracy of a state including at least one of a position, a speed, or anattitude of a mobile object. According to one example embodiment, astate estimation device for estimating a state including at least one ofa position, a velocity, or an attitude of a mobile object. The deviceincludes: an input unit configured to read image data output by acapture unit configured to capture an image of a peripheral area of themobile object and inertia data of the mobile object, the inertia databeing output from an inertial measurement unit installed on the mobileobject; a preprocessing unit configured to extract a feature pointincluded in the image data, track the feature point, and calculate theposition, the velocity, or the attitude of the mobile object based onthe inertia data; a calculation unit configured to calculate a biaserror of the inertial measurement unit by performing bundle adjustmenton the feature point of the image data, the position, the velocity, orthe attitude of the mobile object based on the inertia data; acorrection unit configured to calculate correction data by removing thebias error from the inertia data; and an estimation unit configured toestimate a state including at least one of the position, the velocity,or the attitude of the mobile object based on the correction data.

According to the detailed study by the present inventors, it has beenfound that the estimation error in the attitude change decreases after acertain amount of time has elapsed since the start of attitude changeestimation by the VIO. The reason for this is that the attitudeestimation result obtained from the image data analysis does not includethe bias error of the inertial measurement unit.

In consideration of these, the state estimation device of the presentdisclosure obtains the bias error from the VIO, and estimates the stateof the mobile object based on correction data obtained by removing thebias error from the inertia data.

In this way, when the bias error is estimated by the VIO, and theposition, velocity, and attitude of the mobile object are estimatedbased on the bias error and inertia data, it is possible to theinfluence of the error due to the motion blur and the peripheral mobileobject. Therefore, according to the state estimation device of thepresent disclosure, it is possible to improve the accuracy of estimatingthe state including at least one of the position, the velocity, or theattitude of the mobile object.

According to another example embodiment, a state estimation method forestimating a state including at least one of a position, a velocity, oran attitude of a mobile object. The method includes: an input unitconfigured to read image data output by a capture unit configured tocapture an image of a peripheral area of the mobile object and inertiadata of the mobile object, the inertia data being output from aninertial measurement unit installed on the mobile object; apreprocessing unit configured to extract a feature point included in theimage data, track the feature point, and calculate the position, thevelocity, or the attitude of the mobile object based on the inertiadata; a calculation unit configured to calculate a bias error of theinertial measurement unit by performing bundle adjustment on the featurepoint of the image data, the position, the velocity, or the attitude ofthe mobile object based on the inertia data; calculating correction databy removing the bias error from the inertia data; and estimating a stateincluding at least one of the position, the velocity, or the attitude ofthe mobile object based on the correction data.

In this way, when the bias error is estimated by the VIO, and theposition, velocity, and attitude of the mobile object are estimatedbased on the bias error and inertia data, it is possible to theinfluence of the error due to the motion blur and the peripheral mobileobject. Therefore, according to the state estimation device of thepresent disclosure, it is possible to improve the accuracy of estimatingthe state including at least one of the position, the velocity, or theattitude of the mobile object.

Hereinafter, embodiments of the present disclosure will be describedwith reference to the drawings. In the following embodiments, componentsthat are the same as or equivalent to those described in the precedingembodiment(s) will be indicated by the same reference symbols, and thedescription thereof may be omitted. In the following embodiments, whenonly partial configuration is described in one embodiment, remainingconfiguration may adopt same configurations as that described in thepreceding embodiments. The respective embodiments described herein maybe partially combined with each other as long as no particular problemsare caused even without explicit statement of these combinations.

First Embodiment

The present embodiment will be described with reference to FIGS. 1 to 11. In the present embodiment, an example will be described in which astate estimation device 10 of the present disclosure shown in FIG. 1 isapplied to a vehicle 1 to estimate a position p, a velocity v, and anattitude φ of the vehicle 1 and outputs them to the outside. In thepresent embodiment, the vehicle 1 corresponds to a “mobile object”.

The vehicle 1 is equipped with the state estimation device 10. Thevehicle 1 is equipped with a capture unit 2 and an inertial measurementunit 3 in addition to the state estimation device 10. A part of thestate estimation device 10 may be placed outside the vehicle 1.

The capture unit 2 periodically captures a peripheral area of thevehicle 1, as shown in FIG. 2 . The capture unit 2 outputs image dataobtained by capturing the peripheral area of the vehicle 1. The captureunit 2 is configured by, for example, a camera or the like having aphotoelectric conversion element such as CCD or CMOS. The CCD is anabbreviation for Charge Coupled Device. The CMOS is an abbreviation forComplementary Metal Oxide Semiconductor. The capture unit 2 of thepresent embodiment includes a monocular camera. The capture unit 2 mayinclude a compound eye camera.

The inertial measurement unit 3 is a device that detectsthree-dimensional inertial motion of the vehicle 1. The inertialmeasurement unit 3 outputs translational motion in orthogonal three-axisdirections and rotational motion of the vehicle 1, as inertia data. Theinertial measurement unit 3 includes a gyro sensor 3 a that detects, asthe rotational motion of the vehicle 1, angular velocities ωx, ωy, andωz of the vehicle 1 and an acceleration sensor 3 b that detects, as thetranslational motion of the vehicle 1, accelerations fx, fy, and fz ofthe vehicle 1. In the drawings, the gyro sensor 3 a and the accelerationsensor 3 b may be also referred to as “GYRO SEN” and “ACC SEN”,respectively. The inertial measurement unit 3 of the present embodimentis configured as a small MEMS-based IMU. The MEMS is an abbreviation forMicro Electro Mechanical Systems.

As shown in FIG. 1 , the state estimation device 10 estimates theposition p, the velocity v, and the attitude φ of the vehicle 1 based onthe image data output by the capture unit 2 and the inertia data outputby the inertial measurement unit 3, and outputs the estimation result tothe outside.

The state estimation device 10 is a computer having a controller 20including a processor, a memory 50, and the like. The memory 50 storesprograms, data, or the like for executing various control processes. Thecontroller 20 executes various programs stored in the memory 50.

The state estimation device 10 functions as various functional units byexecuting various programs by the controller 20 and the like. The stateestimation device 10 includes an input unit 22, a preprocessing unit 24,a calculation unit 26, a correction unit 28, and an estimation unit 30.

The capture unit 2 and the inertial measurement unit 3 are connected tothe input unit 22. The input unit 22 reads image data output by thecapture unit 2 and the inertia data output by the inertial measurementunit 3.

The preprocessing unit 24 extracts a feature point FP from the imagedata read by the input unit 22 and tracks the feature point FP, andcalculates the position p, the velocity v, and attitude φ of the vehicle1 based on the inertial data read by the input unit 22.

The preprocessing unit 24 includes an image processing unit 241 thatextracts the feature point FP from the image data and tracks the featurepoint FP. The image processing unit 241, for example, extracts thefeature point FP based on local feature amounts by SIFT, SURF, or thelike, and correlates the feature point FP extracted from a current imageframe by nearest neighbor search or the like to the feature point FPextracted from a previous image frame. The extraction of the featurepoints FP and the correlation of the feature points FP by the imageprocessing unit 241 may be implemented by means different from thosedescribed above.

The preprocessing unit 24 also includes an inertia processing unit 242that calculates the position p, the velocity v, and the attitude φ ofthe vehicle 1 based on the inertia data. The inertia processing unit 242obtains the attitude φ and a rotation matrix Cb of the vehicle 1 by, forexample, integrating the angular velocity ω which is the sensor outputof the gyro sensor 3 a. Further, the inertia processing unit 242 obtainsthe speed v of the vehicle 1 by integrating the product of theacceleration f, which is the sensor output of the acceleration sensor 3b, and the rotation matrix Cb, and integrates the obtained velocity v ofthe vehicle 1 to calculate the position p of the vehicle 1. The inertiaprocessing unit 242 obtains three attitude angles such as roll angle,pitch angle, and yaw angle as the attitude φ by calculation.

The calculation unit 26 estimates various parameters including the biaserror of the inertial measurement unit 3 using the visual inertialodometry. The calculation unit 26 of the present embodiment performs thenonlinear least-squares method called bundle adjustment on the featurepoint FP of the image data, the position p, the velocity v, and theattitude φ of the vehicle 1 based on the inertia data to estimate theposition p, the attitude φ, the velocity v, and the bias error of theinertial measurement unit 3. The calculation unit 26 estimates each ofthe bias error of the gyro sensor 3 a and the bias error of theacceleration sensor 3 b as the bias error of the inertial measurementunit 3.

Specifically, as shown in FIG. 4 , the calculation unit 26 optimizes theresiduals of the image, the IMU, and the prior information by bundleadjustment to estimate the position p, the attitude φ, the velocity v ofthe vehicle 1, and the bias error of the inertial measurement unit 3.

The calculation unit 26 optimizes, as a residual for the image, thereprojection error between the image coordinate system and the worldcoordinate system by bundle adjustment. For example, as shown in FIG. 5, the calculation unit 26 converts the position of the feature point FPin the i-th image frame into the world coordinate system. After that,the calculation unit 26 optimizes, as the residual error related to theimage, the difference between the re-projected position on the imagecoordinate system of the j-th image frame and the position of thefeature point FP in the j-th image frame.

In addition, for example, the calculation unit 26 uses the differencebetween the measurement results of the position p and the orientation φby the inertial measurement unit 3 and the prediction results of theposition p and the attitude φ predicted from the image data as residualsrelated to the IMU, and performs optimization using the bundleadjustment. An inertial data sampling time by the inertial measurementunit 3 is shorter than an image data sampling time by the capture unit2. Therefore, for example, as shown in FIG. 6 , the calculation unit 26uses, as the residual for the IMU, a difference between the predictionresults of the changes in the position p and the attitude φ of thevehicle 1 between the i-th image and the j-th image and the measurementresults of the position p and the attitude φ obtained by integrating theinertial data acquired between the frames.

Further, as shown in FIG. 7 , the calculation unit 26 performs bundleadjustment on not only the most recent information of the image data andthe inertia data, but also prior information. The calculation unit 26optimizes, as residuals related to the prior information, for example,the difference between the position p and the attitude φ of the vehicle1 estimated from the most recent information and the position p and theattitude φ of the vehicle 1 estimated from the previous information.

When new information is added, the calculation unit 26 deletes part ofthe prior information or performs marginalization processing, therebyreducing the load of computation processing and the like. The method ofobtaining the position, the velocity v, and the attitude φ in the visualinertia odometry VIO is also disclosed, for example, in Non-PatentLiterature of “T. Qin, P. Li and S. Shen, “VINS-Mono: A Robust andVersatile Monocular Visual-Inertial State Estimator,” in IEEETransactions on Robotics, vol. 34, no. 4, pp. 1004-1020, August 2018,doi: 10.1109/TRO.2018.2853729”.

Here, FIG. 8 shows analysis results obtained by performing Allandispersion on the angular velocity ω measured by the gyro sensor 3 a andthe angular velocity ω estimated from the attitude φ obtained by thevisual inertia odometry. In FIG. 8 , the two-dot chain line indicatesthe analysis result of the angular velocity ω measured by the gyrosensor 3 a, and the one-dot chain line indicates the analysis result ofthe angular velocity ω estimated from the attitude φ obtained by theVIO.

As shown in FIG. 8 , it was found that the estimation of the angularvelocity ω by the VIO has a larger error within a predetermined timeafter the start of the estimation than the measurement result of thegyro sensor 3 a. A possible reason for this is that image data output bythe capture unit 2 is susceptible to motion blur and the peripheralmobile object, and the error in the attitude φ obtained by the analysisof the image data increases in a short period.

On the other hand, the estimation of the angular velocity ω by the VIOreduces the error over time. The reason for this is considered to bethat the estimation of the angular velocity ω by the VIO is not affectedby the bias error of the gyro sensor 3 a.

On the other hand, as shown in FIGS. 8 and 9 , the error in themeasurement result of the gyro sensor 3 a is small until a certainamount of time has elapsed since the start of measurement, but the errorgradually increases with the passage of time. The reason for this isconsidered to be the accumulation of bias errors over time.

In consideration of these characteristics, the state estimation device10 obtains the bias error from the VIO, and estimates the state of thevehicle 1 based on correction data obtained by removing the bias errorfrom the inertia data. The state estimation device 10 of the presentembodiment includes the correction unit 28 that obtains the correctiondata obtained by removing the bias error from the inertia data, and theestimation unit 30 that estimates at least one of the position p, thevelocity v, or the attitude φ of the vehicle 1 based on the correctiondata.

For example, as shown in FIG. 10 , the correction unit 28 outputs, ascorrection data, data obtained by subtracting the bias error of theinertial measurement unit 3 obtained by the VIO from the inertial dataoutput from the inertial measurement device 3.

The estimation unit 30 calculates the position p, the velocity v, andthe attitude φ of the vehicle 1 based on the correction data, andoutputs the calculation results. The estimation unit 30 integrates theangular velocity ω corrected by the correction unit 28 to obtain theattitude φ of the vehicle 1 and the rotation matrix Cb. Further, theestimation unit 30 obtains the velocity v of the vehicle 1 byintegrating the product of the acceleration f corrected by thecorrection unit 28 and the rotation matrix Cb, and integrates thevelocity v of the vehicle 1 to obtain the position p of the vehicle 1.

Here, FIG. 11 shows analysis results obtained by performing Allandispersion on the angular velocity ω measured by the gyro sensor 3 a,the angular velocity ω estimated from the attitude φ obtained by theVIO, and the angular velocity ω obtained by removing the bias error fromthe angular velocity ω measured by the gyro sensor 3 a. In FIG. 11 , thetwo-dot chain line indicates the analysis result of the angular velocityω measured by the gyro sensor 3 a, and the one-dot chain line indicatesthe analysis result of the angular velocity ω estimated from theattitude φ obtained by the VIO. Further, in FIG. 11 , the solid lineindicates the analysis result of the angular velocity ω obtained byremoving the bias error from the angular velocity ω measured by the gyrosensor 3 a.

As shown in FIG. 11 , the angular velocity ω obtained by removing thebias error from the angular velocity ω measured by the gyro sensor 3 ais different from the angular velocity ω estimated from the attitude φobtained by the VIO, and has the smaller error immediately after thestart of estimation. Further, the angular velocity ω obtained byremoving the bias error from the angular velocity ω measured by the gyrosensor 3 a showed a small error even after a certain amount of timeelapsed, unlike the measurement result of the gyro sensor 3 a.

The state estimation device 10 and the state estimation method describedabove obtain the bias error by the VIO, and estimates the position p,the velocity v, and the attitude φ of the vehicle 1 based on correctiondata obtained by removing the bias error from the inertia data. In thisway, when the bias error is estimated by the VIO, and the position p,velocity v, and attitude φ of the vehicle 1 are estimated based on thebias error and inertia data, it is possible to the influence of theerror due to the motion blur and the peripheral mobile object.Therefore, according to the state estimation device 10 and the stateestimation method of the present disclosure, it is possible to improvethe accuracy of estimating the state including at least one of theposition p, the velocity v, or the attitude φ of the vehicle 1.

Here, the VIO does not sequentially estimate the current state from thepast measurement results and the current measurement results like theKalman filter, but minimizes the error by the nonlinear least-squaresmethod such as bundle adjustment. Although the bundle adjustmentrequires a large computational load, it is characterized by highaccuracy because the bundle adjustment uses multiple data from the pastto the present to obtain the estimation value that minimizes the errorthrough iterative calculations. In particular, the bundle adjustment hasbetter performance than the Kalman filter in terms of resistance todisturbance noise and state estimation using nonlinear functions. Sincethe state estimation device 10 and the state estimation method of thepresent disclosure use the bias error highly accurately estimated by theVIO, it is possible to appropriately correct the inertial data. This iseffective in improving the accuracy of estimating the state including atleast one of the position p, the velocity v, or the attitude φ of thevehicle 1.

Second Embodiment

Next, a second embodiment will be described with reference to FIG. 12 .In the present embodiment, differences from the first embodiment will bemainly described.

As in the first embodiment, when the capture unit 2 includes themonocular camera, the scale estimation error is larger than when thecompound eye camera is used. When this error is large, the accuracy ofestimating the bias error of the acceleration f included in the inertiadata may decrease. Even when the velocity v or the position p of thevehicle 1 is estimated by integrating a value obtained by removing thebias error from the acceleration f in the inertia data, there is apossibility that a sufficient accuracy improvement effect cannot beobtained.

In consideration of this, as shown in FIG. 12 , the state estimationdevice 10 of the present embodiment uses not only the inertia data fromwhich the bias error has been removed, but also the sensor output of thewheel speed sensor 4 to obtain position p and the velocity v of thevehicle 1. The wheel speed sensor 4 includes, for example, a rotaryencoder. The wheel speed sensor 4 outputs a signal corresponding to thenumber of rotations of the wheels of the vehicle 1 to the outside as asensor output.

The estimation unit 30 is directly or indirectly connected to the wheelspeed sensor 4 so as to read the sensor output of the wheel speed sensor4. The estimation unit 30 estimates the velocity v and the position p ofthe vehicle 1 based on the correction data obtained by the correctionunit 28 and the sensor output of the wheel speed sensor 4 as well.

Specifically, the estimation unit 30 integrates the angular velocity ωcorrected by the correction unit 28 to obtain the attitude φ of thevehicle 1 and the rotation matrix Cb. Further, the estimation unit 30obtains the velocity v of the vehicle 1 by integrating the product ofthe acceleration f, which is not corrected by the correction unit 28 butestimated from the output of the wheel sensor 4, and the rotation matrixCb, and integrates the velocity v of the vehicle 1 to obtain theposition p of the vehicle 1.

Others are the same as those in the first embodiment. The stateestimation device 10 and the state estimation method of the presentembodiment can obtain the similar effects to those of the firstembodiment, which are provided by the common configuration or theequivalent configuration to the first embodiment.

Further, the state estimation device 10 of the present embodiment hasthe following features.

(1) The estimation unit 30 of the state estimation device 10 obtains thespeed v of the vehicle 1 based on the correction data and the sensoroutput of the vehicle wheel speed sensor 4 installed in the vehicle 1,and estimates the position p of the vehicle 1 based on the obtainedvelocity v of the vehicle 1. According to this, even when the captureunit 2 includes the monocular camera, it is possible to estimate thevelocity v and the position p of the vehicle 1 with sufficient accuracy.The configuration according to the present disclosure is suitable for aconfiguration in which the monocular camera is used as the capture unit2 and a configuration in which it is difficult to reduce an error in thescale estimation of the camera.

Third Embodiment

Next, a third embodiment will be described with reference to FIGS. 13 to15 . In the present embodiment, differences from the first embodimentwill be mainly described.

In the inertial measurement unit 3, the acceleration sensor 3 b has asimpler structure than the gyro sensor 3 a in terms of sensor structuresuch as MEMS, and the bias change of the acceleration sensor 3 b tendsto be smaller than the bias change of the gyro sensor 3 a. In line withsuch a fact, the bias error estimated by the VIO tends to be lessaccurate with the gyro sensor 3 a than with the acceleration sensor 3 b.

Based on these, as shown in FIG. 13 , the state estimation device 10 ofthe present embodiment estimates the attitude φ of the vehicle 1 byusing the sensor output of the wheel sensor 4 in addition to thecorrection data obtained by correcting the inertia data with used of thebias error obtained by the VIO.

As shown in FIG. 14 , the estimation unit 30 obtains the bias error ofthe gyro sensor 3 a by using the VIO, and calculates a first attitudeangle φ1 indicating the attitude φ of the vehicle 1 based on the outputobtained by correcting the sensor output of the gyro sensor 3 a usingthe bias error.

The estimation unit 30 also calculates the bias error of theacceleration sensor 3 b by the VIO. Then, the estimation unit 30calculates a second attitude angle φ2 indicating the attitude φ of thevehicle 1 based on the output obtained by correcting the sensor outputof the acceleration sensor 3 b with use of the bias error of theacceleration sensor 3 b and a gravitational acceleration obtained fromthe sensor output of the wheel speed sensor 4.

Specifically, the estimation unit 30 removes the translationalacceleration from the sensor output of the acceleration sensor 3 b usinga derivation value of the sensor output of the wheel speed sensor 4.After extracting only the gravitational acceleration, the estimationunit 30 calculates the attitude angle as shown in FIG. 15 to obtain thesecond attitude angle φ2 indicating the attitude φ of the vehicle 1.

Here, since the wheel speed sensor 4 has large quantization noise, it ispreferable to limit the band with a low-pass filter when using the wheelspeed sensor 4. For example, it is preferable that the second attitudeangle φ2 obtained by the estimation unit 30 is smoothed by a movingaverage filter, and only low frequency components are used. In thiscase, although the high frequency component is insufficient, the firstattitude angle φ1 may be used for the insufficient high frequencycomponent.

In consideration of these, the estimation unit 30 passes the firstattitude angle φ1 through a high-pass filter of a complementary filterand passes the second attitude angle φ2 through a low-pass filter of thecomplementary filter, and synthesizes them to estimate the attitude φ ofthe vehicle 1. As for the complementary filters, it is desirable thatthe orders of the cutoff frequencies of the low-pass filter and thehigh-pass filter match.

Others are the similar to the embodiments described above. The stateestimation device 10 and the state estimation method of the presentembodiment can obtain the similar effects to those of theabove-described embodiments, which are provided by the commonconfiguration or the equivalent configuration to the above-describedembodiments.

Further, the state estimation device 10 of the present embodiment hasthe following features.

(1) The estimation unit 30 passes the first attitude angle φ1 through ahigh-pass filter of a complementary filter and passes the secondattitude angle φ2 through a low-pass filter of the complementary filter,and synthesizes them to estimate the attitude φ of the vehicle 1. Inthis manner, it is possible to sufficiently improve the accuracy ofestimating the attitude φ of the vehicle 1 by the configuration ofestimating the attitude φ of the vehicle 1 by using the second attitudeangle φ2 estimated from the sensor output of the acceleration sensor 3 bin addition to the first attitude angle φ1 estimated from the sensoroutput of the gyro sensor 3 a.

Fourth Embodiment

Next, a fourth embodiment will be described with reference to FIGS. 16to 18 . In the present embodiment, differences from the first embodimentwill be mainly described.

When there is a change in the environment around the vehicle 1 (forexample, backlight, tunnel, or the like), it becomes difficult for thecapture unit 2 to perform its intended function. When such an abnormalsituation occurs, for example, as shown in FIG. 16 , the estimation ofthe bias error by the VIO becomes unstable, and it becomes difficult toestimate the position p, the velocity v, and the attitude φ of thevehicle 1 with high accuracy.

In view of this, as shown in FIG. 17 , the state estimation device 10includes an abnormality determination unit 31 that determines whetherthere is an abnormal situation in which it is difficult for the captureunit 2 to perform its intended function.

The abnormality determination unit 31 determines, for example, based onthe image data output by the capture unit 2, whether the imaging unit 2can perform the intended function such as extraction of the featurepoint FP. The abnormality determination unit 31 determines that thesituation is not abnormal when the capture unit 2 can perform theintended function. The abnormality determination unit 31 determines thatthe situation is abnormal when the capture unit 2 cannot perform theintended function.

The correction unit 28 of the present embodiment calculates correctiondata of the inertia data in consideration of the determination result ofthe abnormality determination unit 31. Specifically, when the captureunit 2 is in a normal state in which it can perform its intendedfunction, the correction unit 28 removes the bias error obtained by theVIO from the inertia data to calculate correction data.

On the other hand, when the capture unit 2 is not in the abnormalsituation where the intended function cannot be performed, thecorrection unit 28 calculates the correction data by removing, insteadof the bias error obtained by the VIO, the bias estimation valuepreviously stored in the memory 50 from the inertia data.

Here, the bias error changes depending on stress, temperature, and thelike. However, these do not change much in a short time (for example,about 10 seconds). For this reason, for example, it is desirable thatthe correction unit 28 stores the bias error obtained by the VIOimmediately before the capture unit 2 becomes in the abnormal situationin which it cannot perform its intended function as the bias estimationvalue in the memory 50.

Others are the similar to the embodiments described above. The stateestimation device 10 and the state estimation method of the presentembodiment can obtain the similar effects to those of theabove-described embodiments, which are provided by the commonconfiguration or the equivalent configuration to the above-describedembodiments.

Further, the state estimation device 10 of the present embodiment hasthe following features.

(1) The state estimation device 10 includes the abnormalitydetermination unit 31 that determines whether there is the abnormalsituation where it is difficult for the capture unit 2 to perform itsintended function. The correction unit 28 calculates the correction databy removing the bias error from the inertia data when the determinationof the abnormality determination unit 31 indicates that the situation isnot abnormal. Further, when the determination of the abnormalitydetermination unit 31 indicates the abnormal situation, the correctionunit 28 calculates the correction data by removing, instead of the biaserror calculated by the calculation unit 26, the bias error estimationvalue previously stored in the memory 50 from the inertia data.According to this, even when the abnormal situation occurs in which thecapture unit 2 cannot perform its intended function, it is possible toappropriately continue the estimation of the state of the vehicle 1, asshown in FIG. 18 , for example.

(2) The abnormality determination unit 31 determines whether there isthe abnormal situation based on the image data output by the captureunit 2. According to this, since it is not necessary to add a sensordevice dedicated to abnormality determination of the capture unit 2, itis possible to continue the estimation of the state of the vehicle 1 ina simple manner.

Fifth Embodiment

Next, a fifth embodiment will be described with reference to FIG. 19 .In the present embodiment, differences from the fourth embodiment willbe mainly described.

In the abnormal situation where it is difficult for the capture unit 2to perform its intended function, the preprocessing unit 24 is likely tobe not able to extract the feature point FP of the image data or trackthe feature point FP.

In view of this, the abnormality determination unit 31 of the presentembodiment does not acquire image data from the capture unit 2, butacquires the analysis result of the image data from the preprocessingunit 24 as shown in FIG. 19 and determines whether the abnormalsituation has occurred based on the analysis result. For example, theabnormality determination unit 31 determines that the abnormal situationhas occurred when the preprocessing unit 24 cannot extract the featurepoint FP of the image data at all or when the tracking of the featurepoint FP suddenly becomes impossible.

Others are the similar to those in the fourth embodiment. The stateestimation device 10 and the state estimation method of the presentembodiment can obtain the similar effects to those of the fourthembodiment, which are provided by the common configuration or theequivalent configuration to the fourth embodiment.

Sixth Embodiment

Next, a sixth embodiment will be described with reference to FIG. 20 .In the present embodiment, differences from the fourth embodiment willbe mainly described.

In the abnormal situation in which it is difficult for the capture unit2 to perform its intended function, the accuracy of estimating theattitude φ of the vehicle 1 by the VIO is likely to decrease.Conversely, when the estimation accuracy of the attitude φ of thevehicle 1 by the VIO decreases, the capture unit 2 is likely to be inthe abnormal situation where it is difficult to perform its intendedfunction.

In view of this, as shown in FIG. 20 , the abnormality determinationunit 31 determines whether the capture unit 2 is in the abnormalsituation where it is difficult to perform the intended function, basedon the sensor output of the steering angle sensor 5 installed on thevehicle 1 and the attitude φ of the vehicle 1 obtained by thecalculation unit 26. In the drawings, the steering angle sensor 5 may bealso referred to as “ST ANGLE SEN”. For example, when the attitude φ ofthe vehicle 1 obtained from the sensor output of the steering anglesensor 5 and the attitude φ of the vehicle 1 obtained by the calculationunit 26 deviate from each other and the deviation exceeds a referencevalue, the abnormality determination unit 31 determines that thesituation is abnormal.

Others are the similar to those in the fourth embodiment. The stateestimation device 10 and the state estimation method of the presentembodiment can obtain the similar effects to those of the fourthembodiment, which are provided by the common configuration or theequivalent configuration to the fourth embodiment.

Further, the state estimation device 10 of the present embodiment hasthe following features.

(1) The abnormality determination unit 31 determines whether the captureunit 2 is in the abnormal situation where it is difficult to perform theintended function, based on the sensor output of the steering anglesensor 5 and the attitude φ of the vehicle 1 obtained by the calculationunit 26. In this way, when the sensor output of the steering anglesensor 5 already placed in the vehicle 1 is used to determine whetherthe abnormal situation has occurred, it is not necessary to add thesensor device dedicated to abnormality determination of the capture unit2. Therefore, it is possible to continue the estimation of the state ofthe vehicle 1 in a simple manner.

Seventh Embodiment

Next, a seventh embodiment will be described with reference to FIG. 21 .In the present embodiment, differences from the fourth embodiment willbe mainly described.

In the abnormal situation in which it is difficult for the capture unit2 to perform its intended function, the accuracy of estimating thevelocity v by the VIO is likely to decrease. Conversely, when theestimation accuracy of the velocity v of the vehicle 1 by the VIOdecreases, the capture unit 2 is likely to be in the abnormal situationwhere it is difficult to perform its intended function.

In view of this, as shown in FIG. 21 , the abnormality determinationunit 31 determines whether the capture unit 2 is in the abnormalsituation where it is difficult to perform the intended function, basedon the sensor output of the wheel speed sensor 4 installed on thevehicle 1 and the velocity v of the vehicle 1 obtained by thecalculation unit 26. For example, when the velocity v of the vehicle 1obtained from the sensor output of the wheel speed sensor 4 and thevelocity v of the vehicle 1 obtained by the calculation unit 26 deviatefrom each other and the deviation exceeds a reference value, theabnormality determination unit 31 determines that the situation isabnormal. In the drawings, the wheel speed sensor 4 may be also referredto as “WH SPEED SEN”.

Others are the similar to those in the fourth embodiment. The stateestimation device 10 and the state estimation method of the presentembodiment can obtain the similar effects to those of the fourthembodiment, which are provided by the common configuration or theequivalent configuration to the fourth embodiment.

Further, the state estimation device 10 of the present embodiment hasthe following features.

(1) The abnormality determination unit 31 determines whether the captureunit 2 is in the abnormal situation where it is difficult to perform theintended function, based on the sensor output of the wheel speed sensor4 and the velocity v of the vehicle 1 obtained by the calculation unit26. In this way, when the sensor output of the wheel speed sensor 4already placed in the vehicle 1 is used to determine whether theabnormal situation has occurred, it is not necessary to add the sensordevice dedicated to abnormality determination of the capture unit 2.Therefore, it is possible to continue the estimation of the state of thevehicle 1 in a simple manner.

Eighth Embodiment

Next, an eighth embodiment will be described with reference to FIG. 22 .In the present embodiment, differences from the fourth embodiment willbe mainly described.

In the abnormal situation in which it is difficult for the capture unit2 to perform its intended function, the accuracy of estimating theposition p, the velocity v, and the attitude φ by the VIO is likely todecrease. Therefore, in the abnormal situation where it is difficult forthe capture unit 2 to perform its intended function, the position p, thevelocity v, and the attitude φ of the vehicle 1 obtained by thecalculation unit 26 and the position p, the velocity v, and the attitudeφ of the vehicle 1 estimated by the estimation unit 30 are likely todeviate from each other.

In view of this, as shown in FIG. 22 , the abnormality determinationunit 31 determines whether the capture unit 2 is in the abnormalsituation where it is difficult to perform the intended function, basedon the position p, the velocity v, and the attitude φ of the vehicle 1calculated by the calculation unit 26 and the position p, the velocityv, and the attitude φ estimated by the estimation unit 30. For example,the abnormality determination unit 31 determines that the abnormalsituation has occurred when at least one of the position p, the velocityv, or the attitude φ of the vehicle 1 calculated by the calculation unit26 deviates from the position p, the velocity v, or the attitude φ ofthe vehicle 1 estimated by the estimation unit 30 and the deviationexceeds the reference value.

Others are the similar to those in the fourth embodiment. The stateestimation device 10 and the state estimation method of the presentembodiment can obtain the similar effects to those of the fourthembodiment, which are provided by the common configuration or theequivalent configuration to the fourth embodiment.

Further, the state estimation device 10 of the present embodiment hasthe following features.

(1) The abnormality determination unit 31 determines whether the captureunit 2 is in the abnormal situation where it is difficult to perform theintended function, in other words, determines whether the abnormalsituation in which the capture unit 2 is difficult to perform apredetermined function has occurred based on the position p, thevelocity v, and the attitude φ of the vehicle 1 calculated by thecalculation unit 26 and the position p, the velocity v, and the attitudeφ of the vehicle 1 estimated by the estimation unit 30. According tothis, since it is not necessary to add a sensor device dedicated toabnormality determination of the capture unit 2, it is possible tocontinue the estimation of the state of the vehicle 1 in a simplemanner.

Ninth Embodiment

Next, a ninth embodiment will be described with reference to FIG. 23 .In the present embodiment, differences from the fourth embodiment willbe mainly described.

The bias error of the inertial measurement unit 3 has characteristicsthat change according to the temperature of the inertial measurementunit 3. Therefore, it is preferable that the bias estimation value usedwhen the correction unit 28 calculates the correction data is not afixed value but a variable value that changes according to thetemperature of the inertial measurement unit 3.

In consideration of this, as shown in FIG. 23 , the correction unit 28corrects the bias estimation value according to the temperaturemeasurement result of the inertial measurement unit 3, and removes thecorrected bias estimation value from the inertia data to calculate thecorrection data. For example, when the bias error tends to increase asthe temperature of the inertial measurement unit 3 rises, the correctionunit 28 adds a predetermined value to the bias estimation value storedin the memory 50 when the temperature of the inertial measurement unit 3rises.

Here, the method for measuring the temperature of the inertialmeasurement unit 3 may be the temperature sensor 6 added to the inertialmeasurement unit 3, or may be estimation using the outside airtemperature and the usage conditions of the inertial measurement unit 3.In the drawing, the temperature sensor 6 may be also referred to as“TEMP SEN”. Also, the correction of the bias estimation value is notlimited to the one described above, and may be implemented by othermethods.

Others are the similar to those in the fourth embodiment. The stateestimation device 10 and the state estimation method of the presentembodiment can obtain the similar effects to those of the fourthembodiment, which are provided by the common configuration or theequivalent configuration to the fourth embodiment.

Further, the state estimation device 10 of the present embodiment hasthe following features.

(1) The correction unit 28 corrects the bias estimation value accordingto the temperature of the inertial measurement unit 3, and removes thecorrected bias estimation value from the inertia data to calculate thecorrection data. According to this, even when the abnormal situationoccurs in which the capture unit 2 cannot perform its intended function,it is possible to continue the estimation of the state of the vehicle 1in an appropriate manner.

Other Embodiments

Although the representative embodiments of the present disclosure havebeen described above, the present disclosure is not limited to theembodiments and can be variously modified as follows, for example.

Although the state estimation device 10 of the above embodiments isapplied to the vehicle 1, the present disclosure is not limited to this.The state estimation device 10 can be applied to a mobile object otherthan the vehicle 1.

Although the state estimation device 10 of the above embodimentsestimates the position p, the velocity v, and the attitude φ of thevehicle 1, the present disclosure is not limited to this. The stateestimation device 10 may estimate a state including some of the positionp, the velocity v, or the attitude φ of the vehicle 1.

In the embodiments described above, it is needless to say that theelements configuring the embodiments are not necessarily essentialexcept in the case where those elements are clearly indicated to beessential in particular, the case where those elements are considered tobe obviously essential in principle, and the like.

In the embodiments described above, the present disclosure is notlimited to the specific number of components of the embodiments, exceptwhen numerical values such as the number, numerical values, quantities,ranges, and the like are referred to, particularly when it is expresslyindispensable, and when it is obviously limited to the specific numberin principle, and the like.

In the embodiments described above, when referring to the shape,positional relationship, and the like of a component and the like, it isnot limited to the shape, positional relationship, and the like, exceptfor the case where it is specifically specified, the case where it isfundamentally limited to a specific shape, positional relationship, andthe like, and the like.

The controller and the method described in the present disclosure may beimplemented by a special purpose computer, which includes a memory and aprocessor programmed to execute one or more special functionsimplemented by computer programs of the memory. The controller and themethod described in the present disclosure may be implemented by aspecial purpose computer including a processor with one or morededicated hardware logic circuits. The controller and the methoddescribed in the present disclosure may be implemented by a combinationof (i) a special purpose computer including a processor programmed toexecute one or more functions by executing a computer program and amemory and (ii) a special purpose computer including a processor withone or more dedicated hardware logic circuits. The computer program maybe stored in a computer-readable non-transitory tangible storage mediumas instructions to be executed by a computer.

1. A state estimation device for estimating a state including at leastone of a position, a velocity, or an attitude of a mobile object, thedevice comprising: an input unit configured to read image data output bya capture unit configured to capture an image of a peripheral area ofthe mobile object and inertia data of the mobile object, the inertiadata being output from an inertial measurement unit installed on themobile object; a preprocessing unit configured to extract a featurepoint included in the image data, track the feature point, and calculatethe position, the velocity, or the attitude of the mobile object basedon the inertia data; a calculation unit configured to calculate a biaserror of the inertial measurement unit by performing bundle adjustmenton the feature point of the image data, the position, the velocity, orthe attitude of the mobile object based on the inertia data; acorrection unit configured to calculate correction data by removing thebias error from the inertia data; and an estimation unit configured toestimate a state including at least one of the position, the velocity,or the attitude of the mobile object based on the correction data. 2.The state estimation device according to claim 1, wherein the estimationunit is configured to estimate the velocity of the mobile object basedon the correction data and a sensor output of a vehicle wheel sensorinstalled on the mobile object, and estimate the position of the mobileobject based on the estimated speed of the mobile object.
 3. The stateestimation device according to claim 2, wherein the inertial measurementunit includes a gyro sensor that detects an angular velocity of themobile object and an acceleration sensor that detects an acceleration ofthe mobile object, and the estimation unit is configured to calculate afirst attitude angle indicating an attitude angle of the mobile objectbased on a result obtained by correction of a sensor output of the gyrosensor, the correction being performed by the correction unit, calculatea second attitude angle indicating an attitude of the mobile objectbased on a gravitational acceleration calculated using a result obtainedby correction of a sensor output of the acceleration sensor by thecorrection unit and a sensor output of the wheel speed sensor of themobile object, pass the first attitude angle through a high-pass filterof a complementary filter and passes the second attitude angle through alow-pass filter of the complementary filter, and after passing the firstattitude angle and the second attitude angle, synthesize the firstattitude angle and the second attitude angle to estimate the attitude ofthe mobile object.
 4. The state estimation device according to claim 1,further comprising an abnormality determination unit configured todetermine whether an abnormal situation where the capture unit isdifficult to perform a predetermined function has occurred, wherein thecorrection unit calculates the correction data by removing the biaserror from the inertia data when the abnormal situation has notoccurred, and calculates the correction data by removing, instead of thebias error calculated by the calculation unit, a bias error estimationvalue stored in a memory in advance from the inertia data when theabnormal situation has occurred.
 5. The state estimation deviceaccording to claim 4, wherein the abnormality determination unit isconfigured to determine whether the abnormal situation has occurredbased on the image data.
 6. The state estimation device according toclaim 4, wherein the abnormality determination unit is configured todetermine whether the abnormal situation has occurred based on a sensoroutput of a steering angle sensor installed on the mobile object and anattitude of the mobile object, the attitude being calculated by thecalculation unit.
 7. The state estimation device according to claim 4,wherein the abnormality determination unit is configured to determinewhether the abnormal situation has occurred based on a sensor output ofan vehicle wheel speed sensor installed on the mobile object and a speedof the mobile object, the speed being calculated by the calculationunit.
 8. The state estimation device according to claim 4, wherein theabnormality determination unit is configured to determine whether theabnormal situation has occurred based on the position, the velocity, andthe attitude calculated by the calculation unit and the position, thevelocity, and the attitude estimated by the estimation unit.
 9. Thestate estimation device according to claim 4, wherein the correctionunit is configured to correct the bias error estimation value accordingto a temperature of the inertial measurement unit, and calculate thecorrection data by removing the corrected bias error estimation valuefrom the inertia data.
 10. A state estimation method for estimating astate including at least one of a position, a velocity, or an attitudeof a mobile object, the method comprising: reading image data output bya capture unit configured to capture an image of a peripheral area ofthe mobile object and inertia data of the mobile object, the inertiadata being output from an inertial measurement unit installed on themobile object; extracting a feature point included in the image data;tracking the feature point; calculating the position, the velocity, orthe attitude of the mobile object based on the inertia data; calculatinga bias error of the inertial measurement unit by performing bundleadjustment on the feature point of the image data and the position, thevelocity, and the attitude of the mobile object based on the inertiadata; calculating correction data by removing the bias error from theinertia data; and estimating a state including at least one of theposition, the velocity, or the attitude of the mobile object based onthe correction data.
 11. The state estimation device according to claim1, further comprising a processor that serves as the input unit, thepreprocessing unit, the calculation unit, the correction unit, and theestimation unit.
 12. The state estimation device according to claim 4,further comprising a processor that serves as the abnormalitydetermination unit.